
/**
  * Autores: Bruna Xavier e Bruno Costa
  * 27/05/2012
  *
  * Código para equalizar a velocidade dos motores.
  * Usa os dois speed encoders.
  * Motor esquerda (azul): Porta digital 2
  * Motor direita (verde): Porta digital 3
  *
 **/

#define MAX_VEL 150

#define PIN_MOTOR_LEFT_0 10
#define PIN_MOTOR_LEFT_1 11

#define PIN_MOTOR_RIGHT_0 5
#define PIN_MOTOR_RIGHT_1 6

#define COUNTS_POR_CURVA 10

int left_count;
int right_count;

int left_speed;
int right_speed;

long now;

void setup()
{ 
  Serial.begin(9600);
  
  attachInterrupt(0, inc_left, CHANGE);
  attachInterrupt(1, inc_right, CHANGE);
  
  now = millis();
  motor_forward();
  while(millis() - now < 1000);
  motor_left();
  motor_forward();
  now = millis();
  while(millis() - now < 1000);
  motor_right();
  motor_forward();
  now = millis();
  while(millis() - now < 1000);
  motor_stop();
}

void loop() {}

void setMotorVelocity()
{
  analogWrite(PIN_MOTOR_LEFT_0, left_speed);
  analogWrite(PIN_MOTOR_LEFT_1, 0);
  
  analogWrite(PIN_MOTOR_RIGHT_0, right_speed); 
  analogWrite(PIN_MOTOR_RIGHT_1, 0);
}

void motor_forward()
{
  left_speed = 255;
  right_speed = 255;
  
  setMotorVelocity();
}

void motor_stop()
{
  left_speed = 0;
  right_speed = 0;
  
  setMotorVelocity();
}

void motor_left()
{   
  left_speed = 0;
  right_speed = 255;
  
  setMotorVelocity();
  
  right_count = 0;
  while (right_count < COUNTS_POR_CURVA)
  {
    Serial.println("a");
  }
}

void motor_right()
{   
  left_speed = 255;
  right_speed = 0;
  
  setMotorVelocity();
  
  left_count = 0;
  while (left_count < COUNTS_POR_CURVA)
  {
    Serial.println("a");
  }
}

/**
  * Usando seguindo o esquema de interrupcao
 **/
void inc_left()
{
  left_count++;
}

/**
  * Usando seguindo o esquema de interrupcao
 **/
void inc_right()
{
  right_count++;
}
